#include "arduino.h"

arduino::arduino( int whichOne )
{ 
  numSeq = 0;
  
  if( whichOne == ARDUINO_SCARA )  
  {
    fdScara = serialport_init("/dev/ttyUSB1",115200);
    sleep(2); 
    if(fdScara==-1) 
      perror("arduino SCARA(USB0) nao encontrado");
  }
  else if( whichOne == ARDUINO_OMNI )
      {
	  fdOmni = serialport_init("/dev/ttyUSB0",115200);
	  sleep(2);
	  if(fdOmni==-1) 
	    perror("arduino OMNI(USB1) nao encontrado");
      }   
      else
	perror("parametro invalido para arduino::arduino(int whichOne)");
      
}

arduino::~arduino()
{ 
  close(fdScara);
  close(fdOmni);
}

int arduino::writeScaraServos(int theta1, int theta2, int theta_ef)
{
  int n,i=0;
  bool again = true;
  char msg[16];
  char mensagem[16];
  char aux[6];
  int theta[4];
  char res[10];
  res[0] = '\0';
  msg[0] = '\0';
  aux[0] = '\0';
  
  theta[1] = theta1;
  theta[2] = theta2;
  theta[3] = theta_ef;
  
  mensagem[0] = '<'; //simboliza inicio da msg
  mensagem[1] = '\0';
  
  for( i=1;i<=3;i++)
  {  
    sprintf(aux,"%d",theta[i]);
  
    if ( strlen(aux) == 1 ) // 0 a 9
    {
      strcpy(msg,"00");
      strcat(msg,aux);
    } 
    else 
      if ( strlen(aux) == 2 ) // 10 a 99
      {
	strcpy(msg,"0");
	strcat(msg,aux);
      }
      else
	strcat(msg,aux); // 100 a 999

  strcat(mensagem,msg);
  msg[0]='\0';
  }
	
    mensagem[10] = numSeq+'0';
    mensagem[11] = '>';
    mensagem[12] = '\0';
  
  while( (strcmp(res,"ACK")!=0)  || again )
  {
	  msgEnviada = true;

	  if( numSeq == 0 )
	  {  
	    if( serialport_write(fdScara,"[START]") == -1)
	    { 
	      cout << "Erro em: serialport_write( [START] )"<<endl;
	      usleep(20000);
	      continue;
	    }
	    else
	      cout << "msg enviada: " << "[START]" << endl;
	  }
	  else
	  {
	    if ( serialport_write(fdScara,mensagem) == -1)
	    {
	      cout << "Erro em: serialport_write( mensagem )"<<endl;
	      usleep(20000);
	      continue;
	    }
	    else
	      cout << "msg enviada: " << mensagem << endl;
	  }
  
	  gettimeofday(&t_envio, NULL);
  
	  if( serialport_read_until(fdScara,res,'\n') == -1 )
	  {
	    if( DEBUG )
	      cout << "Erro em: serialport_read_until()"<<endl;
	    
	    numSeq = 0; /* ---RESTARTA O ARDUINO!--- */
	    msg[10]='1';
	    usleep(100000);
	    continue;
	  }
	  else
	  { 
	    timeOutEstourado = false;
	    
	    if( res[0]=='R' && res[1]=='E' && res[2]=='B' && res[3]=='O' && res[4]=='O' && res[5]=='T' && res[6]=='!' )
	    {  
	      arduinoResetou = true;
	      msgEnviada = false;
	      return -1;
	    }
	  }
	  
	  res[strlen(res)-2]='\0';
	  
	  printf("%s\n",res);
	  
	  if( strcmp(res,"ACK")==0 )
	  { 
	    if( numSeq == 9 )
	    {  
	      numSeq=1;
	      again = false;
	    }
	    else
	    {
	      if( numSeq == 0 )
		again = true;
	      else
		again = false;
	      
	    numSeq++;
	    }
	    
	  mensagem[10]=numSeq+'0';  
	  }
  }

msgEnviada = false;

return 0;
}

int arduino::writeCmd( int cmd, char identificador, int whichOne )
{
  int n,fd,i=0;
  char msg[8];
  char mensagem[8];
  bool again = true;
  char aux[8];
  char res[10];
  msg[0] = '\0';
  res[0] = '\0';
  aux[0] = '\0';

  if( whichOne == ARDUINO_SCARA )
    fd = fdScara;
  else if( whichOne == ARDUINO_OMNI )
    fd = fdOmni;
  else
    perror("parametro invalido para arduino::writeServo(...)");

  mensagem[0] = '<';
  mensagem[1] = '\0';
    
  sprintf(aux,"%d",cmd);
  
    if ( strlen(aux) == 1 ) // 0 a 9
    {
      strcpy(msg,"00");
      strcat(msg,aux);
    } 
    else 
      if ( strlen(aux) == 2 ) // 10 a 99
      {
	strcpy(msg,"0");
	strcat(msg,aux);
      }
      else
	strcpy(msg,aux); // 100 a 999

    strcat(mensagem,msg);
	
    mensagem[4] = identificador;
    mensagem[5] = numSeq + '0';
    mensagem[6] = '>';
    mensagem[7] = '\0';
  
  /* Formato da mensagem = <angulo+identificador> */
  
  while( (strcmp(res,"ACK")!=0) || again )
  {
	  msgEnviada = true;
  
	  if( numSeq == 0 )
	  {  
	    if( serialport_write(fd,"[START]") == -1)
	    { 
	      cout << "Erro em: serialport_write()"<<endl;
	      usleep(20000);
	      continue;
	    }
	    else
	      cout << "msg enviada: " << "[START]" << endl;
	  }
	  else
	  {
	    if ( serialport_write(fd,mensagem) == -1)
	    {
	      cout << "Erro em: serialport_write()"<<endl;
	      usleep(20000);
	      continue;
	    }
	    else
	      cout << "msg enviada: " << mensagem << endl;
	 }
	  
	  gettimeofday(&t_envio, NULL);

	  if( serialport_read_until(fd,res,'\n') == -1 )
	  {
	    if( DEBUG )
	      cout << "Erro em: serialport_read_until()"<<endl;
	    
	    numSeq = 0; /* ---RESTARTA O ARDUINO!--- */
	    //msg[5]='0';
	    usleep(100000);
	    continue;
	  }
	  else
	  { 
	    timeOutEstourado = false;
	    
	    if( res[0]=='R' && res[1]=='E' && res[2]=='B' && res[3]=='O' && res[4]=='O' && res[5]=='T' && res[6]=='!' )
	    {  
	      arduinoResetou = true;
	      //msgEnviada = false;
	      return -1;
	    }
	  }
	  
	  res[strlen(res)-2]='\0';
	  
	  printf("%s\n",res);
	  
	  if( strcmp(res,"ACK")==0 )
	  { 
	    if( numSeq == 9 )
	    {  
	      numSeq=1;
	      again = false;
	    }
	    else
	    {
	      if( numSeq == 0 )
		again = true;
	      else
		again = false;
	      
	      numSeq++;
	    }
	    
	  mensagem[5]=numSeq+'0';  
	  }
	  
  }
  
msgEnviada = false;  

return 0;
}



int arduino::readArduino( char identificador, int cmd, int whichOne, char* resposta)
{
  int n,fd,i=0;
  char msg[8];
  bool again = true;
  char comando[6];
  char res[10];
  msg[0] = '\0'; //diz a msg eh uma string vazia
  res[0] = '\0';
  

  if( whichOne == ARDUINO_SCARA )
    fd = fdScara;
  else if( whichOne == ARDUINO_OMNI )
    fd = fdOmni;
  else
    perror("parametro invalido para arduino::writeServo(...)");
  
  msg[0] = '<'; //simboliza inicio da msg
  msg[1] = '\0';
  
  if( cmd < 10) //ang tem um dig
  {  
    msg[1] = '0';
    msg[2] = '0';
    msg[3] = cmd + '0';
  }else if( cmd >=10 && cmd <100 )//dois dig
  {
    msg[1] = '0';
    msg[2] = (int)(cmd/10) + '0';
    msg[3] = cmd%10 + '0';
  }else
    { 
      sprintf(comando,"%d",cmd); // transforma o int em string
      strcat(msg,comando);
    }
  
  msg[4] = identificador;
  msg[5] = (numSeq + '0');
  msg[6] = '>';
  msg[7] = '\0';
  //cout << "msg enviada: " << msg << endl;
 
  while( ((res[0]!= '<') || (res[6]!= '>')) || again )
  {
	  
	  if( numSeq == 0 )
	  {  
	    if( serialport_write(fd,"[START]") == -1)
	      continue;
	  }
	  else
	  {
	    if ( serialport_write(fd,msg) == -1)
		  continue;
	  }
	  
	  cout << "msg enviada: " << msg << endl;
	  
	  msgEnviada = true;
	  gettimeofday(&t_envio, NULL);
	  
	  serialport_read_until(fd,res,'\n'); //int de retorno desprezado
	    
	  msgEnviada = false;
	  res[strlen(res)-2]='\0';
	  printf("%s\n",res);
	  
	  if( strcmp(res,"ACK")==0 )
	  { 
	    if( numSeq == 9 )
	    {  
	      numSeq=1;
	      again = false;
	    }
	    else
	    {
	      if( numSeq == 0 )
		again = true;
	      else
		again = false;
	      
	      numSeq++;
	    }
	    
	  msg[5]=numSeq+'0';  
	  }
	  
  }
 	    
  strcpy(resposta,res);

return 0;
}


int arduino::serialFlush(int whichOne)
{
  int fd;
  int status;
  
  if( whichOne == ARDUINO_SCARA )
    fd = fdScara;
  else if( whichOne == ARDUINO_OMNI )
    fd = fdOmni;
  
  ioctl(fd, TIOCMGET, &status);
  
return status;  
}

